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Abstract 

How do nmltibody systems move in free-fall? For instance, when 
a cat falls, it flips over before it reaches the ground. How does it do 
that? Multibody systems in free-fall move very differently than robots 
which are bolted to the ground. Once a robot with a fixed base stops 
moving, the link positions can be determined by kinematics alone. 
This is not true for a robot or multibody system in free-fall. The 
final link positions of a robot in space depend on the link trajectories 
during the motion as well as its kinematics. Kinematics and dynamics 
are tightly coupled for multibody systems in free-fall. Given these 
difficulties, how can we plan motions for multibody systems in free- 
fall? 

The proposed research will center on several issues necessary to 
plan and execute motions for multibody systems in free-fall. 

1. What motions are possible for a multibody system in free-fall: ? 
Mathematical techniques from nonlinear control theory will be 
used to study the nature of the system dynamics and its possible 
motions. 

2. How can we plan the link motions and joint torques necessary 
to more from one configuration to another? Optimization tech- 
niques will be applied to plan motions. 

3. How can we store precomputed motion plans efficiently? Since it 
is unlikely that motion plans can be computed in real time, pre- 
computation will be necessary. Image compression techniques 
are proposed to compress the precomputed motion data for stor- 
age. 

4. Once a motion is planned, how can the system execute the motion 
faithfully? A linearized controller will be devised to control the 
system while it executes preplanned trajectories. 

Symbolic manipulation techniques will be used in the research (where 
practical) to reduce chances for algebraic errors and to make the ap- 
proach easier to apply to new niultibody systems in free-fall. 

The proposed research applies to a number of activities. Most 
obviously, it can be used to plan motions for robots in space. It can 
be used to plan limb motions to reorient astronauts. The research may 
also be useful to plan the movements of airborne divers, gymnasts, and 
jumpers. 
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Introduction 


1.1 Purpose 

The motion of ground-based robots is reasonably well understood. The un- 
derlying kinematic and dynamic analysis relies on the fact that robots are 
bolted to the floor — which will not move appreciably despite the motions of 
the robots. This is not true for robots in space. More generally, this is not 
true for multibody systems in free-fall. When a multibody system is not 
attached to the earth, its motions are considerably more complex than its 
ground-based counterparts. The dynamics and kinematics become inextrica- 
bly coupled. 

Planning motions for multibody systems in free-fall is more difficult than for 
fixed-base robots largely because of the interaction of the kinematics and 
dynamics. Planning feasible or optimal trajectories will require extensive 
off-line computation and cannot be done in real time. A way is needed 
to precompute and store the possible motions so that the possibilities can 
be retrieved later and used quickly for real-time planning and execution 
purposes. 

The purpose of this research is to develop an encl-to-end system that can be 
applied to a multibody system in free-fall to analyze its possible motions, 
save those motions in a database, and design a controller that can execute 
those motions. A goal is for the process to be highly automated and involve 
little human intervention. Ideally, the output of the system would be data 
and algorithms that could be put in ROM to control the multibody system 
in free-fall. 

This research applies to more than just robots in space. It applies to any 
multibody system in free-fall. This includes astronauts in space, falling mech- 
anisms, athletes in jumps or dives, and airborne gymnasts. 

1.2 Motivation 

To illustrate the complexities and potential of the types of motion that will 
be addressed by this research, consider the lollowing examples. 
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Falling cat problem 
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When a cat is held upside down and dropped, it manages to turn itself 
rightside up before it lands. This well-known phenomenon has long intrigued 
children and scientists. Thomas Kane resolved the question in 1969 with a 
dynamic simulation that showed the motions necessary to turn the cat over 
during its fall [35]. His dynamic simulation involved only two bodies but 
duplicated the complex motion of the cat during the maneuver. 

Astronaut attitude change maneuvers 

Astronauts are taught a series of maneuvers that allow them to change their 
orientation while in free-fall. Again, it was Kane who developed these ma- 
neuvers [36. 37]. For each ol the desired rotations (pitch, roll, and yaw), 
he developed simplified equations of motion and then analyzed them to de- 
termine what cycles of limb motions were necessary to give the desired ori- 
entation change. The result was several cycles ot simple limb motions that 
produce orientation changes about the desired axis. 

Divers, Gymnasts, and Jumpers 

Some of the most complex motions of systems in free-fall occur when spring- 
board divers are in the air [21. 132, 133, 134, 135]. Gymnasts and ath- 
letes perform similar maneuvers while in the air during their activities. The 
movements of high jumpers while off the ground are also complex. The "Fos- 
bury Flop” revolutionized high jumping by improved maneuvers while in the 
air[125]. 

Robot servicing in space 

Using robots to service satellites in orbit is a goal of NASA [56. 101]. Several 
robot designs have been proposer! which involve complex arms attached to 
large bodies with control-moment gyros and thrusters for maneuvering and 
station-keeping [S. 33. 57. 58]. These approaches to motion cont rol have their 
drawbacks. Control-moment gyros are complex and expensive. Thrusters 
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produce plumes which may impinge on delicate equipment. It techniques 
could be developed to plan and execute attitude and configuration changes 
via limb motions, simpler, safer, and less expensive servicing systems could 
be designed. 
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2 Current Approaches (Literature Review) 


Many researchers have addressed various aspects of this research. Relevant 
research is reviewed below. 


2.1 Motion Planning and Control for Robots in free- 
fall 

Only a few researchers have directly addressed the problem of how to plan 
and control motions of a robot or multibody system in tree-lall. 

Kane's work in astronaut maneuvering has already been mentioned. This 
research was reported in 1970-72 [36. 37]. He worked out the equations of 
motion for the human body and simplified them lor the desired rotations ol 
the body trunk. The resulting cyclic motions are interesting and useful but 
very different from the types of motion desired in this research. In any case, 
his work was specific to the human body and did not address the question 
of general configuration change of multibody systems in free-tall. 

Longman. Lindberg. and Zedd considered a robot arm mounted to a satellite 
and developed special kinematics that dealt with the dynamics- kinematics 
interaction problem [57. 54, 58]. They assume the satellite base body con- 
tains reaction wheels to keep it from rotating when the arm moves. Their 
kinematics compensate for the translational movement of the base during 
movements of the arm. Their kinematic simplifications depend on the ab- 
sence of base rotation and do not generalize to multibody systems without 
reaction wheels. 

Vafa and Dubowsky developed the virtual manipulator technique for analyz- 
ing the kinematics and dynamics of robots in space [117. 1 IS. 119. 120. 121. 
80]. They devised a way to construct an imaginary manipulator with dimen- 
sions and inertial characteristics related to the actual system. The motion 
of the imaginary system and the real system are closely related and exactly 
the same for one point of the actual manipulator (such as the end effector). 
Once the position of this common point is determined, the necessary virtual 
manipulator configuration is easily computed and then the corresponding 
joint positions of the actual system can be determined. The dynamics of 
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virtual manipulators also have the advantage that the conservation of lin- 
ear momentum is implicitly integrated and eliminated from the equations 
of motion. l.’n fortunately, this approach does not provide any solutions for 
how to move from one configuration to another. In other words, the virtual 
manipulator approach can be used to determine the final joint positions to 
accomplish some task but is not very helpful in determining the necessary 
joint motions to move from the starting configuration to the final configura- 
tion. They did apply this technique to manipulator motion planning by using 
small cyclic motions to produce small motions of the manipulator base. Al- 
though this approach is useful lor planning the motion ol space manipulator 
end effectors, it has limited usefulness for planning large motions in which 
the entire final configuration is specified (such as in gymnastics). 

Given an end effector position that can be reached, the virtual manipulator 
approach can be used to determine the necessary joint angles of the' space 
manipulator to acheive that position. 

Umetani and Yoshida studied continuous path control of manipulators. They 
devised an extended Jacobian for kinematic analysis of the motion of the 
end-effector [116]. The generalized Jacobian enforces the conservation of 
linear and angular momentum for the space manipulator. They use the 
new Jacobian to plan continuous motions of the end-effector in space and 
simulate them for a OMV (Orbital Maneuvering Vehicle.) This approach 
has similar benefits and limitations as \afa s work because it concentrates 
on end-effector motion. 

Nakamura and Mukherjee also addressed the problem of planning mot ions for 
space robots [71]. Their work deals specifically with the nonholonomic natuie 
of the conservation of angular momentum. They devise kinematics which 
incorporate the linear and angular momentum and of the space manipulator. 
They then devise a controller based on a Lyapunov function. Their approach 
is promising but initial results were disappointing because the controller could 
get stuck during the motion. They solved this problem in their later work 
[72. 73] by designing a bi-directional control algorithm. 1 nfortunately. the 
resulting motions involve unusual cyclic motions and other peculiarities which 
indicate that the mot ion is not very general. 

Sreenath and Krislmaprasad (among others) use mathematical approaches 
to attack the problem of controlling the motion of multibody systems in 
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free-fall [102. 103. 45. 131]. While these methods arc difficult to understand, 
they do appear to offer promising techniques. Unfortunately, their research 
is not very useful to the proposed research because they currently consider 
onlv planar systems. In "Nonlinear Control of Multibody Systems in Shape 
Space,” N. Sreenath indicates that extending their results to three space is 
"definitely non-trivial.” [102. p. 1780]. 

In a recent paper[(>7]. Murray and Sastry use Chow’s theory and Lie brackets 
to determine whether a mot ion is possible for a system subject lo a nonholo- 
nomic constraint linear in velocities. If the motion is possible, they construct 
a path using sinusoidal path segments. They apply their technique to the mo- 
tion of planar systems in free fail and to the nonholonomic vehicle problem. 
Their approach has useful insights and may be applicable to the proposed 
research but has not yet been applied to noil-planar motion. 


2.2 Path Planning for Mobile Vehicles 

An area related to the current research is path planning for mobile robots. 
Much research has been devoted to planning land vehicle motion for indoor 
and outdoor vehicles. Works which considers mobile vehicles as points are 
not considered here since they are irrelevant to the proposed research. There 
are a few researchers who address the nonholonomic nature of vehicles with 
limited steering capabilities. 

Laumond considers a nonholonomic vehicle and proves that it is possible to 
plan collision free paths through a cluttered area by combining sets of small 
cyclic motions [46]. In later work he showed that whenever it is possible 
to plan a jagged path, it is also possible to plan a smooth path for the 
same motion [47]. Barraquand and Latombe addressed similar issues and 
devised planning techniques based on potential field techniques [5]. They 
apply their approach to difficult problems such as parallel parking a vehicle 
with several trailers. They also applied their approach to robot arms with 
many degrees of freedom. Similar research is covered by Jacobs and Canny 
[30. 31]. This type of research has many insights to offer for nonholonomic 
systems. Unfortunately, the nonholonomic constraints due to limited steering 
angles are simpler than the nonholonomic constraints due to the conservation 
of angular momentum. These approaches have not been applied to multibody 


systems in free-fall and it is not dear how applicable they are. 


2.3 Trajectory Planning and Control for Fixed-Base 
Robots 

An extensive amount of research has been done on planning motions lor 
fixed-base robots. A sampling of this research is given in the references 
[6. 17. 34. 40. 22. 23, 78. 64. 74. 75. 91, 89. 97. 98. 100. 105. 111. 126. 127]. 

One of the most promising approaches is presented by Tan and Potts [106. 
107. 108]. Their approach does everything. Their technique is intended 
for fixed-base robots but is general enough to handle multibody systems 
in free-fall. Their technique handles full dynamic nonlinearities, actuator 
limitations, joint constraints (position, velocity, and jerk), avoids obstacles, 
and incorporates an energy objective as well. This approach has not been 
adapted to multibody systems in free-fall but appears useful for this research. 

Another promising approach to planning optimal motions is given by Luus in 
recent research on controlling chemical processes [61]. His approach is based 
on dynamic programming and may be useful for the problem at hand. Luus 
has applied his technique to problems with up to eight nonlinear ordinary 
differential equations and determined optimal controls with limits on input 
variables. 

2.4 Tabular Planning and Control 

Another area relevant to this research is precomputing motion data and stor- 
ing for later use in planning and control. This is sometimes called a tabular 
approach since motion data are precomputed and stored in tables for later 
retrieval in planning or control. Very little has been done in this area.. 

Raibert does use tabular techniques with some success for control of the 
cyclic parts of motion of his on^ legged hopping machine [81, 82. S3]. Tab- 
ular techniques were also proposed by Albus [1, 2]. Hollerbach criticizes 
tabular approaches in general (and these in particular) when he concludes 
that dynamics simualtion codes can be made fast enough to run in real time 
[27]. This criticism is not relevant to the proposed research. It may be pos- 
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sible to simulate the motion ol mult ibody systems in live-tail (aster that real 
time if the torques or forces to apply at each actuator during the motion are 
known before hand. The proposed research is to determine these actuator 
inputs. This cannot be done in real-time using known techniques even on 
super computers. 

2.5 Symbolic Manipulation 

Symbolic manipulation offers researchers many opportunities to improve the 
quaility of their work by producing results much faster than is possible by 
hand, reducing the chance of mathematical errors, and allowing handling of 
more difficult problems. Applying symbolic manipulat ion to robot kinematics 
and dynamics is not new. 

Hussain and Noble used symbolic computation for forward and inverse kine- 
matic analysis of specific robot geometries which assisted the user but still 
required considerable interaction[28]. Direls developed a system for manip- 
ulation of matrices with symbolic entries and used this to analyze robot 
kinematics [16]. Kircanski and Vukobratovic constructed a system using 
FORTRAN-77 to symbolically generate the forward kinematics and Jacobian 
of a robot but not the inverse kinematics solution [41]. Lloyd and Hayward 
applied MAC’SYMA to the same problem and derive kinematics for several 
common robot architectures [55]. Tunstel and \ ira also use MACSAMA to 
construct robot kinematics symbolically as an educational and design aid 
[113]. They also introduce a number of rules (symbolically implemented) 
that simplify the results. 

Many researchers have also developed dynamic equations of motion tor multi- 
body systems symbolically. Liegois and company developed PL/1 software 
to derive equations of motion using a Lagrangian lormulation. Others have 
written FORTRAN programs for symbolic generation of equations of mo- 
tion for multibody systems using various approaches: Newton-Euler [43. 44] 
and Kane's equations [IS]. Other similar work has been done by various 
researchers [7, 11. 24, 26. 29. 49. 50. 62. 66. 76, 1 1 , 69. St>. 90. 94. 95, 96, 109. 
110, 111, 112, 122. 12S. 129. 136. 137] Others have applied similar techniques 
to systems with flexible components [12. 59. 115]. Many of these systems gen- 
erate the equations of motion encoded in a FORTRAN or (. program suited 
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to compiling and running for simulation purposes. Symbolic manipulation 
has also been applied to control applications [94. 104. 107]. 


2.6 Multibody Dynamics 

Multibody Dynamics is a huge field. Many people have developed widely 
varying approaches to the problem of simulating and controlling multibody 
systems. Several references cover Multibody dynamics in detail [4. 1!). 94. 
So, 130]. Others, too numerous to mention, deal with dynamics in gen- 
eral and are applicable to multibody dynamics. Although serious multi body 
dynamics research was done more than SO years ago [20]. the field is not ex- 
hausted. Recent developments include many recursive tech n if pies for inverse 
dynamics with operations counts proportional to the number of elements 
[3, 15. 19, 25. 27. 38, 39. 53. 60. 65. 86, 87. 124. 123]. (Most of these are based 
on recursive Newton- Euler approaches: some are based on operation space 
approaches [19. 53. 86. 87].) The most efficient of these approaches is given 
by He and Goldenberg [25]. Their recursive technique requires 91 (n - 1) -6 
multiplications and 86( ?? — 1 ) — 10 additions, where n is the number of bod- 
ies. The efficiency of these recursive techniques allows the computation of 
joint torques necessary to produce desired motions in real-time for reasonably 
complex svstems. Forward dynamics algorithms are not quite as efficient yet 
[48]. 
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3 Proposed Research 


Before analyzing the proposed research in detail, an overview may be helpful 
to orient the reader. 


3.1 Overview 

The goal of the research is to develop and tost a system which can precom- 
pute. save, and execute motions for multibody systems in free-fall. 1 he basic 
components of the research are listed below. 

1. Analyze motion possibilities 

2. Implement simulation system 

3. Implement symbolic construction of equations of motion 

4. Design optimal controls to accomplish motions 

5. Implement, symbolic generation of optimal control scheme 

6. Precompute motions between selected configurations 

7. Adapt compression techniques to compress motion data 
S. Design linearized motion tracking control scheme 

9. Implement symbolic generation of linearized controller 

10. Use simulation to verify linearized controller 

11. Apply system to example multibody systems 

3.2 Proposed Approach 

3.2.1 Definitions 

Several terms are used in specific ways in this proposal and are defined here. 
The terms appear in italics in the following definitions. 

Configuration (or post) refers to the shape of the body as determined by the 
joint positions. Orientation refers to the attitude of the system with respect 
to some global reference frame. More precisely, orientation refers to the 
attitude of some reference link of the body with respect to a global reference 
frame. If a robot is bolted to the floor, there is no reason to make the 


distinction between configuration and orientation. Once the base ol a robot 
or multibody system is free to move with respect to the global reference 
frame, this distinction becomes useful and important. 

Typical Configurations are configurations ol the multibody system that occur 
often during motions of the system and are useful in studying and planning 
its motions. For instance, a tuck is a typical configuration for divers. lot- 
more detail, see Appendix A. page 32. 

Motions refer to movement from one combination ol configuration ami ori- 
entation to another combination of configuration and orientation. In this 
research, this will be accomplished strictly by joint motions. 

3.2.2 Analyze motion possibilities 

What motions are possible for multibody systems in free-fall? That question 
is central to this research. The possible motions depend on the nature of the 
mechanism, the initial configuration and orientation and the final configu- 
ration and orientation. For instance, a mechanism with one revolute joint 
(like a hinge) has a limited range of motion. It can open and close but the 
axis of the hinge cannot be tilted by opening and dosing the hinge. This is 
because its motion is holonomic. A nonholonomic system has more potential 
motions. Consider a vehicle on the plane with a limited steering angle. The 
front wheel imposes a motion constraint that is nonholonomic. The vehicle 
has three degrees of freedom in the large but only two degrees ot freedom at 
any instant. Yet. by careful maneuvering, any position in the plane can be 
reached. Multibody systems in free-fall must conserve angular momentum 
because they have no external torques acting on them. The conservation 
of angular momentum can be thought ol as a nonholonomic constraint on 
the motion of the system in tree-fall. Depending on the character of the 
angular momentum, a mechanism in tree- fall may be able to move from any 
combination of configuration and orientation to any other combination ot 
configuration and orientation: or it may not — as in the case of a hinge in 
free-fall. Obviously, since no external forces are used, the system center of 
mass will not move either case. 

This research will investigate this issue further and devise tests to be applied 
to determine if each of the desired motions is possible. For example. Ftobe- 



nius' theorem can be applied using Lie brackets to evaluate the uonholonomic 
nature of" the angular momentum (whether it is integrable) [ol. 5. 71. 99. TO]. 
This can be clone symbolically[42] since the angular momentum can be gen- 
erated symbolically. Research will also address the general controllability 
and reachability for these systems. It should be noted that it is very difficult 
to perform this type of research without symbolic manipulation due to the 
complexity of the equations of motion and angular momentum. 


3.2.3 Implement simulation system 

A basic part of the proposed research is a simulation environment in which 
the various components of the research will be implemented and tested. This 
simulation system will allow the user to const met robots from links and joints 
and then simulate kinematics and dynamics of the robots. The simulation 
environment will be used to verify the resulting motion libraries and control 
schemes. An extended description of how the simulation environment can be 
used is included in Appendix A. 

The initial implementation of the simulation environment will handle multi- 
body systems composed of rigid bodies since that is the focus ol this research. 
To be even more useful, the simulation environment should also be able to 
handle flexible members. The software design and implementation will make 
provisions for future expansion in this direction. 

The software approach will be object-oriented and the code will be written 
in an appropriate computer language such as C++, object-oriented Pascal, 
or Ada. An important component of such a system is the graphical display. 
These considerations and the goal of source-code portability indicate that 
C++ and X- Windows might be a good choice. 

3.2.4 Implement symbolic construction of equations of motion 

A number of systems exist for studying the motion of multibody systems. 
These include SD/Fast, SD/Exaet. Autolev. DADS, and ADAMS. Others 
are mentioned in the literature review. These systems simulate multibody 
motions, and some generate C or FORTRAN code for simulation and con- 
trol purposes. Unfortunately, the output ol most of these systems is not 
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directly suitable for further symbolic manipulation (for controls analysis, for 
instance.) 

The proposed system will generate equations of motion in symbolic form 
suitable for further symbolic manipulation. (A few of the systems mentioned 
in the literature review do this.) The resulting symbolic form of the equations 
of motion will be used in three ways. First, the equations of motion will 
be used to generate executable cod*' for simulation and planning purposes. 
Second, the equations of motion will be used to analyze system controllability. 
Third, the equations of motion will be used to construct a linearized controller 
for trajectory tracking purposes. The last two will be done symbolically 
and the resulting symbolic material will be converted to appropriate code as 
necessary. 

The dynamical formulation that will be used has not been determined yet . 
A significant part of the research will involve comparing the various ap- 
proaches and choosing the most appropriate one to implement symbolically. 
Approaches to be compared include Newton-Euler. Lagrange equations (with 
Routh's extensions). Hamilton's canonical equations. Kane's equations, and 
spatial algebra/screw theory approaches. 

To be suitable, the chosen technique of generating equations of motion must 
be suitable for symbolic implementation, and suitable tor efficient simula- 
tions. The symbolic implementation should also apply typical methods to 
improve the efficiency of the code by doing such things as computing common 
subexpressions only once and by precomputing time-invariant terms. 

An issue to be addressed is how to adapt existing recursive approaches to 
multibody dynamics in free-fall. These formulations are satisfactory for sym- 
bolic manipulations for the systems under consideration. Unfortunately, the 
numerical implementations generally depend on the angular velocity of the 
base remaining zero. In free-fall, this is not true. In fact, the angular velocity 
and position of the base depends on the motion of all the joints (due to the 
conservation of angular momentum.) Techniques to handle these unknown 
quantities during the recursions have not been described in the literature 
and will be studied in this research. This may lead to recursive formulations 
for the angular momentum of multibody systems in free-fall. (Note that 
this is not a problem for the symbolic use of recursive formulations since the 
unknown values are carried along as symbolic values in any case.) 
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3.2.5 Design optimal controls to accomplish motions 


Since the idea is to precompute motions, it makes sense to compute the best 
possible motions. Therefore, optimal controls approaches will be used to plan 
the motions. It should be noted that motions produced by this approach will 
be optimal in some sense but that the main goal is to plan motions that are 
feasible and avoid extensive cyclic motions. 

There are several possible approaches to be considered. An optimal control 
scheme based on variational analysis and the maximum principle is a logical 
candidate for computing the multibody motions. Appendix B gives a sample 
of the type of analysis proposed. The analysis actually used must satisfy 
several requirements. It must be simple enough and predictable to imple- 
ment via symbolic manipulation. It must produce the system of executable 
equations (for example, state and costate equations) which are reasonably 
efficient. The analysis in the appendix is given to illustrate the type of ap- 
proach proposed. 

Appendix C' illustrates the application of the optimal control scheme from 
Appendix B to an example. 

Other approaches were mentioned in the literature survey. The technique de- 
scribed by Tan and Potts in "A Discrete Path/Tra jectory Planner for Robotic 
Arms” is intended for fixed-base arms but is adaptable to this current prob- 
lem [107]. It involves constructing a discrete non-linear model of the robot 
dynamics[6S] which can then be used to construct a large non-linear pro- 
gramming problem. The approach is very flexible since it allows constraints 
on joint positions, velocities, jerks, and actuator limitations. It can avoid 
obstacles and will minimize a user specifiable cost function over the path. 

Luus has devised another technique based on dynamic programming which is 
also applicable. In the recent paper "Application of Dynamic Programming 
to High-Dimensional Non-Linear Optimal Control Problems." Luus used dy- 
namic programming to optimize several non-linear problems subject to input 
limitations. In one example, he studied a complex system with eight non- 
linear ordinary differential equations and determined optimum control input 
histories. 


3.2.6 Implement symbolic generation of optimal control scheme 

Once the optimal control scheme is designed, it must be implemented in 
terms of symbolic manipulat ions. An example ol t lie type implementation to 
accomplish this is given in Appendix (A In this example, the optimal controls 
scheme outlined in Appendix B is implemented. Sample results in terms ol 
state and costate equations are given lor example systems. P.xample code is 
also shown which has been generated from these slate and costate equations. 

3.2.7 Precompute motions between selected configurations 

In order to prepare the system for movement between configurations in vari- 
ous orientations, the necessary motions must be precomputed. Tin 1 optimal 
control scheme must be applied to produce the movement data necessary ior 
each motion. This will be implemented in the simulation environment. 

The motion simulations will involve an extensive amount ot computation and 
may require assistance from fast mainframe computers. One advantage ol 
using X- Windows in a networked environment it is quite possible for the 
simulation environment to generate C code for the motion simulation, move 
this to a remote computer (perhaps a super computer), compile the code on 
that computer, run it on that computer, and return the data to the simulation 
environment without user interaction. 
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One premise ol tins research is that the amount ol data generated by the 
motion simulations will not be over- whelming. To validate that assump- 
tion. it is necessary determine how much data will be generated lor various 
situations. Appendix D contains a derivation of the number of data points 
that must be stored as a function of the various parameters. The resulting 
equation is: 

A dp — 3 A o A j A o A f, ( I ) 


'DP = 

Total number o( data points required 

A o = 

Number of relative rotations 

•V; = 

Number of joints or internal DOF 

A'/j = 

Number of data points per variable 


(Number of time steps + 1) 

A> = 

Number of poses (or configurat ions) 


To give some feel for t he amount of data indicated by this equation, consider 
a few examples. Using two configurations and moderately optimistic values 
for the parameters in Equation 1. the amount of storage required tor several 
cases are given in Table 1 (see Appendix D for details). The first example 


Number of Joints. A j 

A data (KB) 

3 

45. S 

6 

91.6 

14 

213.S 


Table 1: Amount of Data Necessary to Store Motions 

uses Nj = 3 and corresponds to a relatively simple mechanism. The second 
example uses Nj = 6 and corresponds to a six degree-of-freedom robot. The 
last example uses A <j = 14 and corresponds (roughly) to a human [134]. 

Although this is a large amount of data, it is within reason. Storing this 
amount of data on hard disks is quite leasible. Storing this amount of data 
in ROM is possible but A p cannot be very large. 




It may be possible to reduce the amount of data to bo stored by only storing 
the joint positions during each motion. Joint velocities and joint torques can 
be computed on the fly by using recursive inverse dynamics formulations. 

3.2.8 Adapt compression techniques to compress motion data 

For each starting configuration and final configuration there will be three 
degrees of freedom in orientation that will be simulated. This can be thought 
of as a vector from the center of a sphere to some point on its surface plus 
an angle about that vector. One of the issues to be examined is how fine 
to subdivide these angles. The grid points must be close enough together 
so that interpolation between nearby motions will produce nearly correct 
results. This will be discussed further in the next section on the motion 
tracking control system. Unfortunately, increasing the number of divisions 
will tremendously affect the amount of number crunching necessary and the 
quantity of resulting data. 

Since the motion simulation will produce a tremendous amount of data, an 
important component of this work will be how to compress it into a motion 
database (or library). Consider the plot for one joint position (or control 
input) over a motion. This is a single simple plot . Now consider a set of 
these for one of the degrees of freedom in orientation. Each plot of the joint 
position can be treated like a scan line of an image so that the set ol plots can 
be thought of as an image. There are three degrees of freedom so the resulting 
data can be thought of as a t wo dimensional array of images. Since the motion 
data can be thought of as images, one approach to compressing this data is 
to apply image compression techniques. The current state of the art in image 
compression for exact reproduction is roughly 3:1 for typical images. In this 
case, exact reproduction is not necessary: techniques exist which give nearly 
lossless single image compressions of roughly 10:1 to 40:1 [32. 10]. W hen 
a number of similar images are compressed, further compression is possible 
by exploiting the similarity between the images — resulting in compression 
ratios of up to roughly 100:1. With this type of compression, it is possible 
to compress an extensive set of data into a reasonable amount of space. It 
is expected that the set of pseudo-images will be relatively similar so that 
compression techniques will be effective. 
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The image compression techniques described typically depend on the im- 
age being composed of integer data with limited range, lor instance. 0-2*)o. 
The joint position and control input data will typically be floating point. 
An issue to be addressed is what level of quantization will allow acceptable 
reconstruction of the joint and control profiles. 

There may be a relationship between the type of compression scheme imple- 
mented and how the system will be used. If the motions are needed often and 
quickly (as it might be for planning), then retreival speed becomes an issue. 
The most effective image compression techniques depend on reconst met ing 
the entire image at once. All that will be needed in t his case is the equivalent 
of one scan line from several different images. Some compression techniques 
may be more efficient for ret reiving one scan line ai a time from an image 
(or set of images). 

The computations for compression will be extensive. This is not necessarily 
a significant problem for an actual application since video compression hard- 
ware exists today which can do the necessary compression at video frame 
rates. 

3.2.9 Design linearized motion tracking control scheme 

The process of compression means the reconstructed joint position and con- 
trol input profiles will not be exactly what they should be. Also, there will 
be uncertainties in the parameters of the actual system. Given the joint 
position and control input profiles necessary to accomplish some configura- 
tion and orientation change how can we persuade the system to actually 
complete the desired motion? Obviously some type oi trajectory tracking 
controller will be necessary. There are a number of possibilities here. One 
is a time-varying linearized control system. Another approach is feedback 
linearization. In the research, various options will be examined. 

The controllability of the time-varying linearized system is an important is- 
sue that will be addressed. In a sense, the linearized control system controls 
the motions in the small at any instant. It will not be fully controllable (in 
general) since it cannot command motions that violate the angular momen- 
tum constraint. 
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A resonablr approach (if the medianism is suitable) is to reduce the order o! 
the system used to determine the planned motion (for instance, by freezing 
some of 1 1 1 < ' joints). Then, during the motion tracking phase, the linearized 
cont roller can use those joints to keep t he system close to t he desired mot ion . 

3.2.10 Implement symbolic generation of linearized controller 

Once the form of the time- varying linearized controller is designed, it should 
not be difficult to use symbolic manipulation to apply it to the equations ol 
motion. In tins way. two implementation problems can be addressed via sym- 
bolic manipulation. First, the system will generate C code to implement the 
linearized feedback control law. This code will not vary during the motion. 
Second, the system will generate C code to compute the time-varying data 
necessary for the linearized controller. This code will be run as necessary to 
update the data in the linearized feedback control code. 

3.2.11 Use simulation to verify linearized controller 

To test the motion data libraries and linearized controller, the system will 
use perform simulations. The system will use standard multibody simulation 
techniques with joint, actuator inputs from the linearized controller and mo- 
tion libraries. These simulations will test many phases of the research. They 
will also give a feel for what kind of accuracy and resolution is necessary in 
the motion database to give adequate control with the linearized controller. 


3.2.12 Apply system to example multibody systems 

To illustrate use of the system and to test it. it will be applied to several 
example multibody systems in free-fall. Useful examples include two body 
systems, typical space robots, and simplified human models. Although hu- 
man motion in free-fall is a desirable application, it may be too ambitious 
for initial applications due to its large number of degrees of freedom. 
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3.3 Expected Results and Contributions 

It should be noted that no single piece of this research is revolutionary. 
At most small extensions from the state ol the art are proposed. W hat 
makes this research unique is the way the components are put together. 
Nobody has yet successfully addressed the end-to-end problem ol how to 
control nniltibody systems in free-fall in real time. I his will be the primary 
contribution of this research. 

Other contributions will include: 


• Extending recursive multibody formulations (or numerical simulations 
of systems in free-fall. 

• Embedding the multibody tree structure in the software objects created X 
to model it. 

• Construction ol a flexible, powerful, and portable simulation environ- 
ment which can be applied to real motion problems. 

• Design and implementation of optimal control for configuration change. 
Using symbolic manipulation to construct the optimal controller. 

• Using image compression techniques to compress motion data. 

• Storing precomputed motions for complex systems for later use. "7 

• Using symbolic manipulation to implement the time- varying linearized 
motion tracking controller. 

• Use of symbolic manipulation for dynamics and controls in one inte- 
grated system. 
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Appendix A. How the Simulation Environ- 
ment Might Be Used 


The following description explains how the simulation environment might be 
used in a step-by-step manner. This sequence described here is not t he only 
wav the svstem can be used, but does illustrate the basic components ol the 
simulation system. 

1. User constructs system in simulation environment. The start- 
ing point of analyzing ihe motion ol the multibody system is lor the 
user to model the multibody system to be analyzed in the simula- 
tion environment. This could be done by direct manipulation (on the 
computer screen) or by reading an appropriate data file. To speciiy 
the multibody mechanism by direct manipulation, the user will select 
the links from a catalog of link shapes, specify (or modify) the link s 
geometric and inertia properties, indicate where any joints would be 
located, and what other links are attached to each joint. The system 
would then create software objects to model the links and joints. Note 
that this would automatically establish the connectivity (or tree struc- 
ture) of the system. The software for each object woidd know how to 
construct the relevant transformations (symbolically and numerically) 
to determine the robot kinematics. Similarly, the object's code would 
also know how to add their components (symbolically and numerically) 
to recursive dynamic formulations. 

2. User chooses typical configurations At this point, the user will 
choose typical configurations or posts tor the multibody system. The 
tvpical poses will be selected to put the multibody system in vari- 
ous useful or desirable configurations. For example, il the system is 
a human, a typical configuration might be a straight body with arms 
extended. In [21]. Cliff Frohlich gives nine different human body config- 
urations that are commonly used by divers and trampolinists. Whether 
the bodv is upside down or right side up is not import ant in specifying 
the configuration. The typical poses will probably include only con- 
figuration information (such as joint positions) and will not include 
joint velocities. Including initial and final velocities in the typical con- 
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figurations will increase com put at ion and storage requirements by an 
unreasonable degree. The system will be able to plan individual mo- 
tions with initial and final velocities but these will not be included as 
part of the precomputation part of the research. This may limit the 
usefulness of the precomputation and storage aspect of the research to 
human motion since large initial and final velocities are often part of 
athlet ie mot ions. 

The typical configurations may also be selected to simplify the system 
dvnamics. For instance, the pose might pul the wrists into a neutral 
position. During the planned motion, these joints might not be used 
to reduce the dimensionality of the problem. 

3. System automatically constructs equations of motion. After 
the description of the system is entered, the system will generate the 
equations of motion in symbolic form. The simulation system will be 
able to deal with the dynamics of the system in three ways. First, 
it will be able to simulate the multibody system dynamics directly 
using standard recursive approaches. Second, it will be able to generate 
the equations of motion in a symbolic form. Third, it will be able to 
simulate the multibody dynamics by using software code generated 
from the symbolic representations of the equations of motion. 

4. System automatically generates optimal controls. Once the 
equations of motion are generated in symbolic form, the optimal con- 
trol law for reconfiguration will be derived symbolically. This is why 
it is important to generate the equations of motion in symbolic form. 
This will also include generation of code to verify whether the motion is 
possible (from the analysis of nonlinear controllabiltiy and reachability 
analysis). 

5. System simulates optimal motions. Once the equations of motion 
have been generated and the optimal control scheme has been con- 
structed. these will be used to simulate optimal motions for orientation 
changes between the typical configurations. During each simulation 
the basic data of control inputs and states during the motion will be 
saved. The goal is to simulate the motions from any configuration to 
aiiy other configuration in any other orientation where such motions 
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are possible. Hopefully, the number of typical coiifigurat ions will be 
small. (If the number of typical coiifigurat ions is large, the amount of 
computation and resulting data may be excessive.) 

(>. System compresses the resulting simulation data into library 
of motion data. I he preceding step will generate a large amount 
of data. The resulting motion data will compressed using image com- 
pression techniques to construct a database ol motion data (or motion 
library). 

7. System designs a linearized controller to execute the maneu- 
vers in the motion library. After the system constructs the motion 
library, the library can be used to plan and execute motions. How- 
ever. the data compression techniques will introduce some errors into 
the reconstructed motion profiles due to quantization and othei effects. 
This, along with imperfect modeling, indicate a motion tracking con- 
troller will be necessary. At this point, the simulation environment will 
use the symbolic version of the equations ol motion to symbolically gen- 
erate the necessary linearized controller to allow the system to execute 
a retreived motion profile. This linearized controller will be converted 
from symbolic form to usable software code for simulation purposes. 

8. User uses system to simulate various motions. Once the previous 
steps have been completed, the simulation environment can be used to 
simulate motions and test the motion tracking controller. This could 
be the goal of the entire system. Consider how such a system could be 
used: 

• The user could simulate possible motions just to see what they 
look like and what types of control inputs are necessary. 

• The system could be used to verify the linearized controller b\ do- 
ing simulations with precomputed motion data and a perturbed 
system. The control inputs could go into a true dynamic simula- 
tion to verify t he results. 

• A diving coach could use the system to construct a new di\e se- 
quence and show it to divers in a movie form. This would in- 
volve using several intermediate poses and splicing together the 


34 


necessary motions. (It might also involve general ing specilic new 
motions or new motion libraries.) The system would take the sets 
of joint position profiles and construct a longer sequence to show 
how the motion would look. Individual joint motions could be iso- 
lated to show the diver what to do and when. It is even possible 1 
that this system could be used to discover maneuvers that have 
newer been thought of Indore. 

• Similar techniques could be used with astronauts to train them to 
do combined configuration and orientation maneuvers. 

• The system could be used to construct a motion library and track- 
ing controller for an orbital servicing robot. This might involve 
generating new motion libraries tailored to specific tasks. The 
resulting data and code could be put into ROM tor use on orbit. 

One of the goals of this work is to reduce the amount of user interaction 
necessary. Ideally, steps 1 and 2 would be the only steps the user would have 
to supervise. In reality, some interaction will probably be necessary during 
some of the other steps as well. 
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Appendix B. Sample Optimal Control 

Analysis 


Suppose that we have a nonlinear system such as a multibody robot in Iree- 
I'all and we would like to choose a set of control inputs to move the system 
from one configuration to another. Start by putting the equations of motion 
of the system into t he following form (this can bo done symbolically from the 
equal ions of mot ion ): 

■^x = f(x) + g(x)u ('2) 

(If 

where x is a vector describing t lie state* and velocities ol ibe multibody system 
and u is a vector o! available control inputs (such as control torques at each 
joint). This form was chosen since the equations of motion for multibody 
systems can generally be put into this form. 

The initial configuration. x(to) = x 0 i is known and the goal is to use the 
control inputs to move the system into the final configuration. x(/y) = X/. 
( where tj is unspecified ). The requirement that the system acheive t he desired 
terminal state can be formulated in the terminal constraint: 

= x — X/ = 0 ( ; 1) 

The terminal constraint is adjoined to the terminal cost (which is zero so far) 
by using a vector of constant multipliers, u: 

$[x(//).f/] = v T iP = i/ r (x-x/) (4) 

The motion should minimize some combination of time required for the mo- 
tion and control effort of the actuators during the motion. A suitable cost 
function is: 



J = *[x(</M/]+ / Ldt 
Jt 0 

(5) 

where: 

1 x 

L = a + ~u T Bu 

(6) 


0 < a < 1 

(7) 


r mi -«) > o if i = j 
B,J "lo if i±j 

(8) 
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The constant a is the ratio between the eon Hiding goals of minimizing the 
time required for t he motion and minimizing t lie control effort of t he actua- 
tors during the motion. If a = 0, then time is of no concern. If a = 1 then 
control effort levels are ol little concern. Note that B is positive definite so 
B~ x exists. Also, since B is diagonal. B~' = B~ T . 

Following the typical optimal controls approach, the previous constrained 
problem can be converted to an unconstrained optimization. This is done 
by constructing a modified cost functional which enforces the equations of 
motion by adjoining the equations of motion with lagrange multipliers. A: 

.) = <h[x( //).//]+ f [/, + A'f-x + f + gu )j (I I ( • M 

To simplify the problem, the Hamiltonian of the system at some instant in 
time is introduced: 

H - L + A T (f + gu) * (10) 

Which means the modified cost functional is: 

j = *[x(//).//]+ r \h — A T xj (It (11) 

J 1 0 


Taking the total variation of ./ and integrating by parts results in: 


Sj = 





'OH % 


; T 


OH c 

dx + — — <^u 
clu 


(It 

( 12 ) 


Note that the variation with respect to A has been omitted since it leads 
back to the equations of motion. To force the variation of the modified cost 
functional. <*>./ , to vanish, we choose: 


A T (t f ) 

0H_ 

du 

fc\t=t 0 


on 

dx 

0*1 


o 


(13) 

(14) 
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0 


(15) 

(16) 


OH e eT 

= — f + f 7 

Ox 


OH 

Ot 



+ Ou + 


OH 

Oi 


(23) 


(24) 


The Hamiltonian is autonomous so —H = ^ = 0. This means that H is 
constant on the optimal trajectory. Its constant value must ho the same as 
its final value. H j = 0. Therefore. II — 0 on the optimal trajectory. 

The final value of the coslate vector. A. is determined from l lie terminal cost : 


A(//) = A ; 


0<\> 






Ox 

0_ 

Ox 

^-{v T [x - X/)) 
Ox 


J i=i f 


lT 


J >=>l 


V 


(25) 


(26) 


(27) 


This indicates that the final value of the costates are unknown constants. In 
order to determine their values, consider the Hamiltonian at the final time. 
Substitute u = -B~ x g r . \ = vA s = f(x(//)). and g/ = g(x(//)) into H: 


H = 


a + ^u r 5u + A r (f + gu) 


(29) 


J (=(/ 


= e + i(-j5- , g;i/) r /?(-B- , g;i/) + */ r (f/ + g / (-(5- , g;i/)) (3oj 

= « + \v T %fB - T + u T {f - v T % s B~'% T ; V (31) 

= a + v T i s - ^i/ r (g/£ -1 gj)i/ (32) 

At the final time. H is: 

H = a + i/ T fj - ^I/ T (g/Z? _, gj)i/ = 0 (33) 
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These choices guarantee that the resulting A ami u |>ro<luce a stationary 
value of j . For J to be minimized. the second gradient must be positive 
definite: _ _ 


Urn 

H IIU 


> 0 


where the subscripts represent partial derivatives. I li is can be implemented 
symbolically. If this is satisfied, then the resulting controls will be optimal. 
See [9. p.oO]. for a detailed derivation of this requirement. Note that //„„ = B 
which was chosen to be positive definite. 


Applying these results to the problem at hand gives: 


on _ -r 

Ox ~ 


on 

o u 


= o 



=> u r j? + A r g = 0 


(IS) 


( 10 ) 


Equation (IS) gives the differential equations for the costate vector. A. Solv- 
ing Equation (19) for u gives the optimal control law: 


u = -B~ x g T X 


( 20 ) 


Since the terminal time. //. is not specified, it is a free parameter. Treating ij 
as a free parameter produces a modified total variation of the cost functional. 
bj. The previous analysis and choices force all the terms to vanish except 
for the variation due to possible changes in the final time: 

M f ( 21 ) 

'='/ 


bJ = 


0<i> „ 

ir + H 


See [9. p.72]. for a detailed derivation of this requirement. Since $ is au- 
tonomous. ^ = 0 and therefore Hj — 0. 

The Hamiltonian. H. is constant as can be seen by taking its derivative with 
respect to time: 



OH OH- OH. OH 
— x + -tt-A + — u + — 
Ox OX Ou Ot 


•>oi 
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However, since 11 = 0 during the entire motion. a similar statement is true 
at the intial time: 


II = a + n T f 0 - -H T {g 0 B 'g 0 r )fi = 0 


(34) 


where n = A(/ 0 ). fo = f(x(/ 0 )). and g 0 = g(x(/u)). Not c that ix is not known 
but that fu and go are both known. (Note that if this is satisfied at the 
initial time, the optima] control guarantees that tlx 1 similar requirement will 
be satisfied at the final time.) 

This is a quadratic form in fi. It describes a multi-dimensional surface. 
Mathematically, this equation can have either no solutions, a unique solut ion. 
or many solutions. 

A Newton- Raphson style scheme can be used to find a solution for p il one 
exists. Consider the change in 11 to a small change in //, near a solution: 

0 H 

H 0 (ni.ft 2 ,/i, + A/q./q+i //„) = H 0 {fi) + + Oififl-Vy) 


If ft is near a solution, then Hoifii-f '2 f'i + A//,./q+ j /'„) ~ 0 and 

the O(ftJ) terms are nearly zero so the resulting equation is: 

0 ~ Hoifi) + "——-A//, (36) 

Oft, 


This equation can be solved for A ft,: 

v H 0 (p) 



Then ft, can be improved: 

(/^i)ll<«' — T -A/6" 


(37) 


(38) 


This is a scalar equation for one //,. The values of can be determined as 
follows: 

= (39) 

For computational efficiency, the n update equations of the form of Equa- 
tion (38) can be applied in parallel. 


40 


Some experimental ion may be required to choose tin 4 weighting values <i and 
bj to produce reasonable trajectories with reasonable joint actuator levels. 

In summary, the system has a set of first-order ordinary different ial equations 
for state and costate. It is a two point boundary problem where initial and 
final values exist for the state. The initial values of the costate are unknown 
but candidates can be found using the Newfon-Raphson type approach just 
described. This problem can be attacked by a shooting technique. Errors at 
the end can be used to improve the guess of the inital coslate values. The 
only modfication needed to this is that the initial costate values must be 
further modified so that they satisify the quadratic form. 


Appendix C. Applied Optimal Controls 

Example 

Appendix C.l. Description 

The purpose of t his example is to demonstrate the type ol symbolic ma- 
nipulation techniques that can be applied to generate optimal controls laws. 
Obviously, this requires that the equations of motion are available in symbolic 
form. 

Appendix C.2. MACSYMA Usage Descriptions and 

Code 

The following description of the function OPTCONT is from the 
file OPTCONT. US AGE: 


The function OPCONT will take an array of first order ordinary 
differential equations with a cost functional and it will derive 
and return the optimal control, the costate equations, the 
Hamiltonian, the Hessian of the system. To use this function, the 
dynamic system must be put in the form of a list of first order 
differential equations of the form: 

dx 

— = f ( x , u) 
dt 

USAGE: OPTCONT (odes ,L,x_name,u_name) ; 


INPUTS : 


odes : list of first order ordinary differential equations, eg, 
[dxl/dt = f 1 ( x , u) , dx2/dt = f2(x,u), ...] 
describing the dynamics of the system. 

L : the cost functional of the system (in terms of x and u) ; 
[scalar function of x,u] 
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x_name 


: list of names of the states used in odes and L. 

Must be in the same order and in one-to-one corrspondence 
with the variables in the left hand sides in the odes. 

u_name : list of names of the control inputs used in f and L. 


OUTPUTS : 


A list composed of: 

costate : List of the costate first-order ode equations in terms 
of the original state variable names and new multiplier 
variables, Li. 

costate.names : a List of the newly introduced costate names 

u_opt : List of Optimal controls 

H : The Hamiltonian of the system, H = L + LT*f [scalar 
function] 

H_hess : The hessian of H, 

H_hess = [ Hxx Hxu ] 

[ Hux Huu ] 

This can be used to determine if the generated control 
inputs are optimal, [(n+m) x (n+m) MATRIX of scalar 
functions] 

NOTE: The list costate contains variables of the form L1,L2,L3... 

This function uses KILL on all of these it uses, so existing 
variables with names of this form will be destroyed. 

By: Jonathan M. Cameron 

The MACSYMA code for the function OPTCONT follows: 

OPTCONT (odes , L , x_name ,u_name ) : = 

BLOCK ( [ n : Length(odes) , /♦ Number of states */ 

m : Length(u_name) , /* Number of controls */ 

Lambda, U.Eqns, Hx, Hu, i, j, 
costate, u_opt, H, H.hess] , 
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/* Construct list of lambdas */ 

Lambda : □ , 
for i : 1 thru n do 

Lambda : append (Lambda, [concat ( 'L, i)] ) , 

Apply ( * KILL , Lambda) , 

/* Form the Hamiltonian */ 

H : L, 

for i : 1 thru n do 

H : H + LambdaCi] *RHS(0des [i] ) , 

/* Construct costate equations */ 

Hx : [] , 

for i : 1 thru n do 

Hx : append (Hx , [dif f (H , x_name [i] )] ) , 
costate : [] , 
for i: 1 thru n do ( 

Depends (LambdaCi] ,T) , 

costate : append( costate , [dif f (lambda [i] ,T) = -Hx[i]]) 

), 


/* Solve for the optimal controls */ 

Hu : □ , 

for i: 1 thru m do 

Hu : append (Hu , [diff (H ,u_name[i] )] ) , 

U_Eqns : □ , 
for i : 1 thru m do 

U_Eqns : append (U_Eqns , [0 = Hu[i]]), 
u_opt : Solve(U_Eqns,u_narae) , 

/* Generate the Hessian */ /* H_hess = [Hxx Hxu] */ 

H.hess : ZEROHATRIX(n+m,n+m) , /* [Hux Huu] */ 

/* Do the Hxx block */ 
for i: 1 thru n do 
for j : 1 thru n do 

H_hess[i, j] : diff(Hx[j], x.nameCi]), 

/* Do the Hxu block */ 
for i: 1 thru n do 
for j : 1 thru m do 

H.hessCi, j+n] : di«(Hu[j], x_name [i] ) , 

/* Do the Hux block */ 
for i : 1 thru m do 
for j : 1 thru n do 

H_hessCi+n, j] : diff(Hx[j], u_name[i]). 
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/♦Do the Huu block ♦ / 
for i : 1 thru m do 
for j : 1 thru m do 

H_hess [i+n, j+n] : diff(Hu[j], u_name[i]), 

/♦ Return the results ♦/ 

[costate, lambda, u.opt , H, H.hess] 

)$ 

The following description of the function SIMPCONT is from the 
file SIMPCONT. US AGE: 

SIMPCONT is a function which will take lists of state and costate 
first order ordinary differential equations, the optimal control, 
and other outputs of OPTCONT and will substitute in the optimal 
control in the state DDEs and then solve as many of the ODEs as 
possible. The resulting system is ready to simplify via boundary 
conditions . 

USAGE: 

SIMPCONTC st at erodes , costate.odes , u.opt , state.names ,costate_names ) ; 


INPUTS: 


state. odes : List of first order ordinary differential equations 
[dxl/dt = fl(x,u), dx2/dt = f2(x,u), ...] describing 
the dynamics of the system. 

costate.odes : List of first order ordinary differential equations 
derived for optimal control by OPTCONT 

u.opt : list of optimal controls derived by OPTCONT 

state.names : list of the names of the states 

costate.names : list of the names of the costates (generated by 
OPTCONT) 


OUTPUTS: 



A list composed of: 


i M 

I Si 


new_system : List of the new simplified set of state and costate 
equations 

new_names : List of the names of the states or costates in 
new_system 

constants : List of the new constants generated by solutions of 
DDEs 


By: Jonathan M. Cameron 

The MACSYMA code for the function SIMPCONT follows: 

SIMPCOHTCstate, costate, u_opt , state_name ,costate_name) := 

BLOCK (C n : Length (state) , 
m : Length(u_opt) , 
new.system : □ , 
new.name : □ , 
const_num : 0, 
constants : □ , 
i, ii, soln] , 

/♦ Substitute the optimal controls into the state equations */ 
for i:l thru m do 

for ii : 1 thru n do 

stateCii] : LHS(state[ii] ) = subst(u_opt, RHS(state[ii] )) , 

/* check each of the costate ODEs and try to solve them */ 
for i: 1 thru n do ( 

soln : ode2(costate[i] , costate_name [i] ,t) , 
if soln # 'FALSE then ( 

soln : subst(concat( ’ C, const_num) //.C.soln) , 
constants : append ( [concat( *0, const _num)] , constants), 
const_num : const_num + 1 , 

/* Do substitutions with soln to eliminate the costate */ 
for ii:l thru n do ( 

stateCii] : LHS(state [ii] ) s subst (soln, RHS( state [ii] )) , 
costate[ii] : LHS ( costate [ii] ) = subst(soln,RHS(costate[ii] )) 

>, 

for ii : 1 thru length(new_system) do 
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new_system[ii] : 

LHS(new_system[ii] ) = subst (soln, RHS(new_system[ii] ) ) 

) 

else ( 

new_system : append(new_system, [costateCi]] ) , 
new.name : append (new_n am e , [costate_name[i]] ) 

) 

). 

/* check each of the state ODEs and try to solve them */ 
for i: 1 thru n do ( 

soln : ode2(state [i] , state.name [i] ,t) , 
if soln # ’FALSE then ( 

soln : subst(concat( ’C, const _num) ,7.C,soln) , 
constants : append ( [concat( ’ C, const _num)] , constants), 
const_num : const_nura + 1 , 

/* Do substitutions with soln to eliminate the state */ 
for ii: 1 thru n do ( 

state [ii] : LHS(state[ii] ) = subst (soln,RHS(state [ii] ) ) , 
costate Cii] : LHS ( costate [ii] ) = subst(soln,RHS(costate[ii] ) ) 
), 

for ii : 1 thru length(new_systera) do 
new_system[ii] : 

LHS(new_system[ii] ) = subst (soln, RHS(new_system [ii] )) , 

/* Add this solution to the system */ 
new_system : append(new_system, [soln]), 
new^name : append (new_naroe , [state_name[i]] ) 

) 

else ( 

new_system : append(new_system, [state[i]]), 
new_name : append ( new_name , [state_name [i] ] ) 

) 

>, 

/♦ Return the results */ 

De cl are ( constats, constant), 

[new_system, new_name, constants] 

)$ 
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Appendix C.3. 


Sample MACSYMA Session Output 


The following out put is from a MACSA MA session using the functions OP T- 
CONT and S1MPC0NT on a simple problem. 

(C3) load ( opt cont ) ; 

Batching the file USERD1 : [CAMERON . PROP] opt cont .mac ; 60 
Batchload done. 

(D3) USERD1 : [CAMERON . PROP] optcont .mac ; 60 


(C4) load(simpcont) ; 

Batching the file USERD1 : [CAMERON. PR0P]simpcont.mac;33 
Batchload done. 

(D4) USERD1 : [CAMERON . PROP] simpcont. mac; 33 

(C5) kill(x,v,u); 

(D5) DONE 


(C6) dependsC [x,v,u] ,t) ; 

(D6) [X(T) , V (T) , U(T)] 


(C7) state.eqns : [diff (x,t)=v,diff (v,t)=u] ; 

dX dV 

(D7) [— = V, — = U] 

dT dT 


(C8) state_names : [*x,’v]; 

(D8) IX V] 
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(C9) control_names : [’u]; 


i l ! 



(D9) 


[U] 


(CIO) L : 0.5*u~2; 
(DIO) 


2 

0.5 U 


(Cll) /* Find the optimal control and costate equations */ 
results : optcont(state_eqns ,L , state.names ,control_names)$ 


(C12) costate.eqns : results [l]; 


dLl dL2 

(D12) [ = 0, = - LI] 

dT dT 


(C13) costate.names : results [2]; 

(D13) [LI, L2] 


(C14) opt_control : results [3]; 

(D14) [U = - L2] 


(C15) H : results [4]; 
(D15) 


2 

L1V+0.5U +L2U 


(C16) Hessian : results [5]; 


(D16) 


[ 0 0 0 ] 
[ ] 
[ 0 0 0 ] 
[ ] 
[ 001 ] 
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(C17) /* Solve as many of the ODEs as possible */ 
results : 

simpcont (state_eqns , costate_eqns , opt_control , state_names , costate_names)$ 
DUBO: [MACSYMA_412.0DE]ode2.fas ; 1 being loaded. 


(C18) system : results [l] ; 


2 2 
CO T CO T 

(D18) [X = T ( Cl T + C3) + C2 , V = Cl T + C3] 

2 2 


(C19) names : results [2]; 

(D19) CX, V] 

(C20) constants : results[3]; 

(D20) [C3, C2 , Cl, CO] 

(D21) DONE 

The next step depends on the problem to be solved. In this case, it is not hard 
to apply inital and final state values to resolve the resulting constants. In this 
example, the costate and state differential equations were solved completely. 
This will not happen with the type of systems to be considered in this re- 
search. In general, some differential equations will be produced. In any case, 
it is not difficult to take the results of SIMPCONT and use MACSNMA 
to convert it to C or FORTRAN code for simulation purposes. Symbolic 
manipulation systems such as MAC’SN MA and Mathematics have powerful 
capabilities to generate program code. A function to take the results of SIM- 
PCONT and generate code could also perform various optimizations such as 
computing common terms only once. 
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Appendix D. Movement Library Size 

Requirements 


In order to validate the premise that the amount of data that will be saved 
is not too excessive, an estimate is presented in this appendix. 

The motions will move the system from one combination of orientation and 
configuration to another combination ol orientation and configuration. Sup- 
pose the goal is to move front one typical configuration to anothei ivpital 
configuration. Since the system is in free-fall, the final configntal ion Inis 
three degrees of attitude freedom with respect to the starting configuration. 
Think of this as the points on a unit sphere and another degree ol freedom 
about a line from the center of the unit sphere to the points on the surface 
of the sphere. In order to tesselate the unit sphere, a procedure based on 
constructing geodesics from icosahedrons can be used [13]. The degree ol 
the tesselation is Q. To tesselate the unit sphere so that there is an angle of 
approximately o between vertices, and: 

Q = int [arctan(2)/o] ( 4 ^) 

where Q = degree of tesselation (If) 

o = approximate angle between vert eces 

Therefore, the total number relative orientations that must be considered foi 
moving from one configuration to another is: 

X a = Number of relative orientations (42) 

= ^-[lOQ' 2 + 2] < 4; *) 

For each orientation, the joint positions, velocities, torques must be saved 
over the motion. So the number ol data points for one motion is. 


A 


.\1 


Number of data points per motion 

3 XjXd 

where Xj = Number of joints 

Xp = Number of data points per variable 


(44) 

(46) 

(46) 


of 


Multiplying A'p <m<l Ay gives the number of (Iain points necessary to store 
the motions from one configuration to another. 


X\IP = Number of data points to move from 
one configuration to another 

= Ao-V\/ 

= ; 3A 


( 48 ) 

(49) 
(•■ 0 ) 


Now assume there are Ap typical configurations. The total numbei data 
points to for motions from any configuration to any other is: 

A /> p = Total number of data points ( ; >1) 



= 3 .Vo A j A d [Ap(Ap — 1) + Ap] (A3) 

= 3Ao A j Ap A p (34) 


where the term ( A ?) in Equation (52) gives the number of pairs of config- 
urations. The factor of 2 is necessary because the motions for each pair ol 
configurations could considered in either direction. The next term. Ap. ac- 
counts for motions from one configuration back to the same configuration in 
a different orientation. 

Given a data compression ratio of Cr to 1 and assuming that the floating 
point value for each data point can be quantized to bits (or a b\te). the 
amount of data (in KB or kilobytes) is: 


Ap.irt 


Total amount 

A pp 


1024C’/? 


KB 


of data in K Bytes 


( 33 ) 

(56) 


To give some feel for the amount of data indicated by these equations, con- 
sider a few examples. For 10° orientation resolution. Q «; 6 and A o = 13.032. 
For each example A p = 10* Ap = 2. and C /? = 100. The results aie gi\en in 
Table 1 on page IT. 


